#include "coro.h"
#define MAX_TASK 10
#define MAX_WAIT 20
typedef unsigned int  TCB[11];
extern int my_setjmp(TCB);
extern void my_longjmp(TCB,int);

TCB env[MAX_TASK];

int current = 0;
typedef struct{
	unsigned char* nonsens[8];
	unsigned char* LR;
	unsigned char* SP;
}frameStack;


void yield(int m, int i)
{
        my_setjmp(env[m]);
        my_longjmp(env[i],1);
}



unsigned int currentTask = 0;
void sched(unsigned int tid)
{
	if( my_setjmp(env[currentTask]) == 0 )
	{
		currentTask = tid;
		my_longjmp(env[tid],1);
	}
	
	
	#if 0
        longjmp(env[1],1);
        while(1)
        {
        }
	#endif
}

/*Return the id of the routine
*/
int lastCo = 1;
int coro_spawn(fun_ptr f,unsigned char* stack)
{
	volatile fun_ptr task = f;
	if(my_setjmp(env[lastCo])==0)
    {
				frameStack*  jump_buf = (frameStack*)(env[lastCo]);
				jump_buf->SP = stack;
			  jump_buf->LR = (unsigned char* )f;
			  return lastCo++;
			
    }
    else
    {
      //  printf("something wrong\r\n");
			while(1);
    }
}


int delayArray[MAX_TASK];
int WaitArray[MAX_WAIT];
void coroInit(void)
{
	int i = 0 ;
	for(i=0;i<MAX_TASK;i++)
	{
		delayArray[i] = -1;
	}
	for(i=0;i<MAX_WAIT;i++)
	{
		WaitArray[i] = 0;
	}
}

/*
* return -1 when wait timeout
* return 0 when event occur
*/
int WaitTimeredEvent(int wait_id,  int timeout)
{
	delayArray[currentTask] = timeout;
	waitEvent(wait_id);
	if( delayArray[currentTask] == -1 )
  {
		 WaitArray[wait_id]  = 0;
		 return -1;
	}
	else{
		delayArray[currentTask] = -1;
		return 0;
	}
		
}

void waitEvent(int wait_id)
{
	if(wait_id >=MAX_WAIT)
	    return;
	WaitArray[wait_id] = currentTask;
	sched(0);
}

void wakeEvent(int wait_id)
{
	int taskid = WaitArray[wait_id];
	if(wait_id >=MAX_WAIT)
	    return;
	if( taskid == 0 )
	    return;
	WaitArray[wait_id] = 0;
	sched(taskid);
}

void fork(int task_id)
{
	delayArray[currentTask] = 1;
	sched(task_id);
}

void delayMs(int delay)
{
	delayArray[currentTask] = delay;
	sched(0);
}

void delayTimerMSCheck()
{
	unsigned int i = 0;
	for(i=0;i<MAX_TASK;i++)
	{
		if( delayArray[i] >= 0 )
		{
			if(delayArray[i]-- == 0 )
			{
				sched(i);
			}
		}
	}
}



